#ifndef _OBSTACLEDETECTORUTIL_H
#define _OBSTACLEDETECTORUTIL_H

#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <Eigen/Dense>

#include "module_base/util.h"
#include "obstacle_detector/util.h"

namespace obstacle_detector
{
struct Region
{
	int id;
	float center_x;
	float center_y;
	float length;
	float width;
	float yaw;  //这个框旋转几度可以转到横平竖直的程度
	float line_k;
	float line_b;
	bool is_merge;
	Eigen::MatrixXf vertexs; //在横平竖直状态下，左上角为第一个点，顺时针排列
	std::vector<int> point_analysis;
	Region()
	{
		center_x=0;
		center_y=0;
		length=0;
		width=0;
		yaw=0;
		vertexs.resize(2,4);
		point_analysis.resize(4);
		is_merge=false;
		id=0;
	}

	void set_info(Eigen::MatrixXf _vertexs)
	{
		vertexs=_vertexs;
		auto diff1=vertexs.col(1)-vertexs.col(0);
		auto diff2=vertexs.col(2)-vertexs.col(1);
		auto center=vertexs.rowwise().mean();
		yaw=atan2(diff1(1),diff1(0));
		length=sqrt(pow(diff1(1),2)+pow(diff1(0),2));
		width=sqrt(pow(diff2(1),2)+pow(diff2(0),2));
		center_x=center(0);
		center_y=center(1);
	}

};

	
struct Object
{
	int label;
	std::string id;
	Eigen::MatrixXf bbox;
	Eigen::MatrixXf global_bbox;
	Eigen::Vector2f velocity;
	float yaw;
	Eigen::Vector2f center;
	Eigen::Vector2f dim;
	float global_yaw;
	Eigen::Vector2f global_center;
	Eigen::Vector2f global_dim;
	int check_count;
	int lost_count;
	std::vector<Eigen::Vector2f> history_locs;
	std::vector<Eigen::Vector2f> history_v;
	Eigen::MatrixXf predict_locs;
	int road_id;
	Eigen::MatrixXf min_bbox;
	// int point_num;
	Object(std::string _id,int _label,const Eigen::MatrixXf &_bbox,
		const module_base::Location &loc)
	{
		id=_id;
		label=_label;
		bbox=_bbox;
		Eigen::Matrix2f rotation;
		rotation<<cos(-1*loc.yaw),sin(-1*loc.yaw),
			   -1*sin(-1*loc.yaw),cos(-1*loc.yaw);
		global_bbox=rotation*bbox.block(0,0,2,8);
		Eigen::Vector2f loc_center;
		loc_center<<loc.x,loc.y;
		global_bbox=global_bbox.colwise()+loc_center;

		Eigen::MatrixXf bottom_box=bbox.block(0,0,2,4);
		auto diff=bottom_box.col(1)-bottom_box.col(0);
		yaw=atan2(diff(1),diff(0));
		center=bottom_box.rowwise().mean();

		dim<<(bottom_box.col(1)-bottom_box.col(0)).norm(),(bottom_box.col(3)-bottom_box.col(0)).norm();


		Eigen::MatrixXf global_bottom_box=global_bbox.block(0,0,2,4);
		auto global_diff=global_bottom_box.col(1)-global_bottom_box.col(0);
		global_yaw=atan2(global_diff(1),global_diff(0));
		global_center=global_bottom_box.rowwise().mean();

		global_dim<<(global_bottom_box.col(1)-global_bottom_box.col(0)).norm(),(global_bottom_box.col(3)-global_bottom_box.col(0)).norm();
		velocity<<0,0;
		check_count=0;
		lost_count=0;
		road_id=-1;
		min_bbox=Eigen::MatrixXf::Zero(2,4);
	}

	Object(std::string _id,int _label,const Eigen::MatrixXf &_bbox,
		const Eigen::MatrixXf &_global_bbox)
	{
		id=_id;
		label=_label;
		bbox=_bbox;
		global_bbox=_global_bbox;
		
		Eigen::MatrixXf bottom_box=bbox.block(0,0,2,4);
		auto diff=bottom_box.col(1)-bottom_box.col(0);
		yaw=atan2(diff(1),diff(0));
		center=bottom_box.rowwise().mean();

		dim<<(bottom_box.col(1)-bottom_box.col(0)).norm(),(bottom_box.col(3)-bottom_box.col(0)).norm();


		Eigen::MatrixXf global_bottom_box=global_bbox.block(0,0,2,4);
		auto global_diff=global_bottom_box.col(1)-global_bottom_box.col(0);
		global_yaw=atan2(global_diff(1),global_diff(0));
		global_center=global_bottom_box.rowwise().mean();

		global_dim<<(global_bottom_box.col(1)-global_bottom_box.col(0)).norm(),(global_bottom_box.col(3)-global_bottom_box.col(0)).norm();
		velocity<<0,0;
		check_count=0;
		lost_count=0;
	}

	Object(const Eigen::MatrixXf &_global_bbox)
	{
		global_bbox=_global_bbox;
		Eigen::MatrixXf global_bottom_box=global_bbox.block(0,0,2,4);
		auto global_diff=global_bottom_box.col(1)-global_bottom_box.col(0);
		global_yaw=atan2(global_diff(1),global_diff(0));
		global_center=global_bottom_box.rowwise().mean();
		global_dim<<(global_bottom_box.col(1)-global_bottom_box.col(0)).norm(),(global_bottom_box.col(3)-global_bottom_box.col(0)).norm();
	}

	void add_loc2history()
	{
		history_locs.push_back(global_center);
		if (history_locs.size()>50)
		{
			history_locs.erase(history_locs.begin());
		}
	}
	void add_v2history(const Eigen::Vector2f &v)
	{
		history_v.push_back(v);
		if (history_v.size()>50)
		{
			history_v.erase(history_v.begin());
		}
	}
};

void read_calib(std::string folder,float *all_transform);

Eigen::MatrixXf generate_bbox(const Eigen::Vector3f &center,
	const Eigen::Vector3f &dim,float yaw);

void save_points(const int16_t* points, const uint16_t* points_order,
	int point_num,const module_base::Location &loc);
void save_points(const float* points, int point_num,
	double stamp) ;

void read_points(int16_t* points, uint16_t* points_order,int &point_num,
	module_base::Location &loc);
void save_bbox(std::vector<Object> &all_objects,
	std::string file_name);
void read_bbox(std::vector<Object> &all_objects,
	std::string file_name);

void save_occupancy_grid(const module_base::Location &loc,
	uint8_t *occupancy_grid,int x_size,int y_size);
void read_occupancy_grid(module_base::Location &loc,
	uint8_t *occupancy_grid,int x_size,int y_size);
void get_color(int label,float *color);

}

#endif 